#include "servo.h"
#include <iostream>

Servo::Servo()
{
  mainfd = 0;   /* File descriptor for the port */
  min = 500;
  max = 5500;
  step = 50; // Decrease step size for finer control.
  mainfd = open_port("/dev/ttyACM0");
  
  if(mainfd != EXIT_FAILURE)
  {
    config_port(mainfd);
    curAngle = 90;
    toAngle(curAngle);

    foundServo = true;
  }
  else
  {
    foundServo = false;
  }
}

Servo::~Servo()
{
  if(foundServo)
    close_port(mainfd, "/dev/ttyACM0");
}

void Servo::incUp()
{
  curAngle++;
  
  if(curAngle > 180)
    curAngle--;
  else  
    toAngle(curAngle);
}

void Servo::incDown()
{
  curAngle--;
  
  if(curAngle < 0)
    curAngle++;
  else
    toAngle(curAngle);
}

void Servo::toAngle(double angle)
{
  double fraction = angle/180;
  double pulse = fraction*(1950-1050)+1050;
  put(0,pulse*4); 
}

/*
* 'open_port()' - Open the serial port.
* Returns the file descriptor on success or -1 on error.
*/
int Servo::open_port (char portName[]) {
   /*
    * The open function is built as follows...
    *    1. The name of the serial port as a C-string (i.e. char *) eg. /dev/ttyso
    *   2. Configuration options...
    *      O_RDWR - we need read and write access
    *      O_CTTY - prevent other input like keyboard from affecting what we read
    *      O_NDELAY - we don't care if the other side is connected (some devices don't explicitly connect)
    */
   int fd;   /* File descriptor for the port */
   fd = open(portName, O_RDWR | O_NOCTTY | O_NDELAY);
   
   if (fd == -1) {  /* Could not open the port */
      fprintf(stderr, "\nopen port: Unable to open %s - %s\n\n", portName, strerror(errno));
      return EXIT_FAILURE; // EXIT
   }
   else {
      printf("\nopen port: port %s has been opened correctly.\n\n", portName);
      printf("fd = %i\n\n", fd);
   }
   
   return (fd);
}

/*
* 'config_port()' - Configure the serial port.
*/
void Servo::config_port(int mainfd) {
   /*
    * Configuring the port is harder in Linux over that of other OS's.
    * There is more bit masking involved in order to change a single option.
    * While there are convienient functions that can be used to set the speed of the port, other options,like 
    * parity and number of stop bits, are set using the c-cflag member of the termios struct, and require bitwise
    * operations to set the various settings.
    * Linux is also capable of setting the read time-out values. This is set using the c-cc member of the termios 
    * struct which is actually an array indexed by defined values.
    */
   // Create the struct
   struct termios options;
   
   // Get the current settings of the serial port.
   tcgetattr (mainfd, &options);

   // Set the read and write speed to 19200 BAUD.
   // All speeds can be prefixed with B as a settings.
   cfsetispeed (&options, B9600);
   cfsetospeed (&options, B9600);

   // Now to set the other settings. Here we use the no parity example. Both will assumme 8-bit words.

   // PARENB is enabled parity bit. This disables the parity bit.
   options.c_cflag &= ~PARENB;
   
   // CSTOPB means 2 stop bits, otherwise (in this case) only one stop bit.
   options.c_cflag &= ~CSTOPB;

   // CSIZE is a mask for all the data size bits, so anding with the negation clears out the current data size setting.
   options.c_cflag &= ~CSIZE;

   // CS8 means 8-bits per work
   options.c_cflag |= CS8;
}

/*
* 'close_port(int mainfd, char portName)' - Close the serial port.
*/
void Servo::close_port(int mainfd, char portName[]) {
   /* Close the serial port */
   if (close(mainfd) == -1) {
      fprintf(stderr, "\n\nclose port: Unable to close %s - %s\n\n", portName, strerror(errno));
      exit(EXIT_FAILURE); // EXIT
   }
   else {
      printf("\n\nclose port: The port %s has been closed correctly.\n\n", portName);
      exit(EXIT_SUCCESS);
   }
}

/*
* This allows for a set system instruction pause. Note: 1000ms = 1 sec.
*/
void Servo::waitMS (int ms) {
   clock_t endwait;
   endwait = clock() + ms * CLOCKS_PER_SEC/1000;
   while (clock() < endwait);
}

void Servo::put(int servo, int angle) {
   int buffSize = 4;
   unsigned char buff[buffSize]; // The data to be sebt to the servo controller.
   
   // DWORD is not a standard C datatype.  Typically it represents a double word.
   // See typedef at top
   DWORD len;
   
   unsigned short int temp;
   unsigned char pos_hi, pos_low;
   
   
   pos_hi = (angle & 0x1F80) >> 7;   
   
   pos_low = angle & 0x7F;
   
  buff[0] = 0x84;
  buff[1] = servo;
  buff[2] = angle&0x7F;
  buff[3] = (angle>>7)&0x7F;
   
   /* 
    * Write data to the serial port.
    *    mainfd is the file descriptor of the serial port.
    *    buff is a pointer to the data that we want to write to the serial port.
    *   buffSize is the amount of data that we want to write.
    */
   if(foundServo)
     int wordsWritten = write(mainfd, &buff, buffSize);
}
